/** @file
 */
#include "esp301-control.h"
#include <sstream>

#if defined (_WIN32) || defined( _WIN64)
#define DEVICE_PORT "COM1"	// COM1 for windows
#elif defined (__linux__)
#define DEVICE_PORT "/dev/ttyS0"	// ttyS0 for linux
#endif

/** RS232 Baudrate */
#define BAUDRATE 19200

/** Timeout for reading data from RS232 interface */
#define TIMEOUT_MS 5000

/** Damping wait of the actuator */
#define STEADY_STATE_WAIT 200

esp301_control::esp301_control ()
{
  theoretical_displacement = 0;
  //connect();
}

void
esp301_control::preset ()
{
  goHome ();
  setLimit ();
}

bool
esp301_control::connect ()
{
  ret = Open (DEVICE_PORT, BAUDRATE);	// Open serial link at 115200 bauds
  if (ret != 1)			// If an error occured...
    throw "Error while opening port. Permission problem ?";	// ... display a message ...

  ret = WriteString ("1MO;TB\r");
  if (ret != 1)
    {
      throw
	"Cannot connect to device. Check whether the cable is connected, and turn on the ESP301 controller.";
      return 0;
    }
  ret = ReadString (buffer, '\r', RS232_BUFFER_MAX, TIMEOUT_MS);	// Read a maximum of 128 characters with a timeout of 0.3 second
  // The final character of the string must be a line feed ('\n')

  if (ret == 0)
    {
      throw "Timeout reached. Cannot identify device ID.";	// If not, print a message.
      return 0;
    }
  return 1;
}

void
esp301_control::goHome ()
{
  ret = WriteString ("1OR;1WS0\r");
}

void
esp301_control::setLimit ()
{
  std::stringstream format_instruction;
  format_instruction << "1SL" << ESP301_LOWER_LIMIT << ";1SR" <<
    ESP301_UPPER_LIMIT << "\r";

  ret = WriteString (format_instruction.str ().c_str ());

  chkError ();
}

esp301_control::~esp301_control ()
{
//ret=WriteString("1MF\r");
  Close ();
}

float
esp301_control::getPosition ()
{

  ret = WriteString ("1WS0;1TP\r");	// Send the command on the serial port
  if (ret != 1)			// If the writting operation failed ...
    throw "Error sending command (1TP)";	// ... display a message ...

  // Read a string from the serial device
  ret = ReadString (buffer, '\r', RS232_BUFFER_MAX, TIMEOUT_MS);	// Read a maximum of 128 characters with a timeout of 0.3 second
  // The final character of the string must be a line feed ('\n')

  if (ret == 0)
    throw "TimeOut reached. No data received !";	// If not, print a message.

  std::stringstream format_decimal (buffer);

  float displacement;
  format_decimal >> displacement;
  return displacement;
}

void
esp301_control::setTolerance (float tol)
{
  std::stringstream format_decimal;
  format_decimal << "1FE" << tol << "\r";

  ret = WriteString (format_decimal.str ().c_str ());	// Send the command on the serial port
  if (ret != 1)			// If the writting operation failed ...
    throw "Error sending command (1FE*)";	// ... display a message ...

  chkError ();
}


bool
esp301_control::moveTo (double x)
{
  if (x < ESP301_LOWER_LIMIT || x > ESP301_UPPER_LIMIT)
    {
      throw "Requested position out of hardware range";
      return 0;
    }

  std::stringstream format_instruction;
  format_instruction << "1PA" << x << ";1WS" << STEADY_STATE_WAIT << "\r";

  ret = WriteString (format_instruction.str ().c_str ());
  if (ret != 1)
    throw "Cannot send command (1PA*)";

  chkError ();

  theoretical_displacement = x;
  return 1;
}

void
esp301_control::chkError ()
{

  ret = WriteString ("TB\r");
  if (ret != 1)
    {
      throw "Cannot send command (TB)";
      return;
    }

  ret = ReadString (buffer, '\r', RS232_BUFFER_MAX, TIMEOUT_MS);	// Read a maximum of 128 characters with a timeout of 0.3 second
  // The final character of the string must be a line feed ('\n')

  if (ret == 0)
    throw "TimeOut reached. No data received! (Expect 0)";	// If not, print a message.

  if (buffer[1] != '0')
    throw (buffer + 1);
}

bool
esp301_control::moveBy (double dx)
{

  return moveTo (theoretical_displacement + dx);

}
